
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       lpfilter.h
  * @author     baiyang
  * @date       2021-7-7
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    float output;      //滤波输出
    float cutoff_freq; //截至频率
    float alpha;
} LowPassFilt;

typedef struct
{
    Vector2f_t output;  //滤波输出
    float cutoff_freq;  //截至频率
    float alpha;
} LowPassFilt_vec2f;

typedef struct
{
    Vector3f_t output;   //滤波输出
    float cutoff_freq;   //截至频率
    float alpha;
} LowPassFilt_vec3f;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
float lpf_apply1(LowPassFilt* pLPFObject, float in);
float lpf_apply2(LowPassFilt* pLPFObject, float in, float dt);

Vector2f_t lpf_apply1_vec2f(LowPassFilt_vec2f* pLPFObject, const Vector2f_t* in);
Vector2f_t lpf_apply2_vec2f(LowPassFilt_vec2f* pLPFObject, const Vector2f_t* in, float dt);

Vector3f_t lpf_apply1_vec3f(LowPassFilt_vec3f* pLPFObject, const Vector3f_t* in);
Vector3f_t lpf_apply2_vec3f(LowPassFilt_vec3f* pLPFObject, const Vector3f_t* in, float dt);

static inline float lpf_get_output(LowPassFilt* pLPFObject) { return pLPFObject->output;}
static inline Vector2f_t lpf_get_output_vec2f(LowPassFilt_vec2f* pLPFObject) { return pLPFObject->output;}
static inline Vector3f_t lpf_get_output_vec3f(LowPassFilt_vec3f* pLPFObject) { return pLPFObject->output;}

void lpf_set_cutoff1(LowPassFilt* pLPFObject, float cutoff_freq);
void lpf_set_cutoff2(LowPassFilt* pLPFObject, float cutoff_freq, float dt);

void lpf_set_cutoff1_vec2f(LowPassFilt_vec2f* pLPFObject, float cutoff_freq);
void lpf_set_cutoff2_vec2f(LowPassFilt_vec2f* pLPFObject, float cutoff_freq, float dt);

void lpf_set_cutoff1_vec3f(LowPassFilt_vec3f* pLPFObject, float cutoff_freq);
void lpf_set_cutoff2_vec3f(LowPassFilt_vec3f* pLPFObject, float cutoff_freq, float dt);

static inline void lpf_reset(LowPassFilt* pLPFObject, float output) { pLPFObject->output = output;}
static inline void lpf_reset_vec2f(LowPassFilt_vec2f* pLPFObject, Vector2f_t* output) { pLPFObject->output = *output;}
static inline void lpf_reset_vec3f(LowPassFilt_vec3f* pLPFObject, Vector3f_t* output) { pLPFObject->output = *output;}

static inline void lpf_reset2(LowPassFilt* pLPFObject) { pLPFObject->output = 0.0f;}
static inline void lpf_reset2_vec2f(LowPassFilt_vec2f* pLPFObject) { vec2_zero(&pLPFObject->output);}
static inline void lpf_reset2_vec3f(LowPassFilt_vec3f* pLPFObject) { vec3_zero(&pLPFObject->output);}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



